import rclpy,cv2,cv_bridge,numpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist
from std_msgs.msg import Int32

from std_msgs.msg import String  # 新增导入

class Follower(Node):
    def __init__(self):
        super().__init__('line_follower')
        self.get_logger().info("Start line follower.")

        self.bridge = cv_bridge.CvBridge()

        self.image_sub = self.create_subscription(Image, '/image_raw1', self.image_callback, 10)
        self.cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', 10)
        self.pub = self.create_publisher(Image, '/process_image', 10)
        self.twist = Twist()

        #self.hkh_sub = self.create_subscription(String, 'hkh_data', self.hkh_callback, 10)
        self.hkh_sub = self.create_subscription(Int32, "/sign4return",self.hkh_callback, 10)
        self.cmd_vel_enabled = True 
        self.flagZ = 0
        self.flag = 0
        self.flagA = 0
        self.cnt = 0
        self.cnt2 = 0
        self.cnt3 = 0
        self.cnt4 = 0
        self.cnt5 = 150

        self.last_err = 0
        self.twist.linear.x = 0.05
        self.twist.angular.z = 0.0
        self.cmd_vel_pub.publish(self.twist)
    def image_callback(self, msg):
        if not self.cmd_vel_enabled:
            return

        image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
        kernel = numpy.ones((7,7), numpy.uint8)  # 5x5 的全为1的矩形结构元素
        #kernexian = numpy.ones((3,3), numpy.uint8)  # 5x5 的全为1的矩形结构元素
        gray_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
        h, w, d = image.shape
        search_top = int(h / 2 - 50 )
        search_bot = int(h-100 )
        self.flagZ = 0
 
        _,binary_image = cv2.threshold(gray_image, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU)
        #膨胀
        dilated_image1 = cv2.dilate(binary_image, kernel, iterations=1)
    
        dilated_image1[0:search_top, 0:w] = 255
        dilated_image1[search_bot:h, 0:w] = 255
        dilated_image1[search_top:search_bot, 0:100] = 255
        dilated_image1[search_top:search_bot, 540:w] = 255

        binary_image2 = cv2.bitwise_not(dilated_image1)#fz
        contours, _ = cv2.findContours(binary_image2, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)#画框

        if self.flagA == 1:
            self.cnt4 += 1

        self.cnt5 += 1
        # 遍历轮廓
        for contour in contours:
            
            # 计算轮廓的边界框
            x, y, w, h = cv2.boundingRect(contour)
            # 计算轮廓的面积（像素数量）
            area = cv2.contourArea(contour)
            
            # 仅选择足够大的框-------避障
            if area > 9000 and self.cnt4 < 70 and self.cnt2 <=40:  # 设置一个最小尺寸阈值，可以根据实际情况调整
                # 在原始图像上绘制边界框
                self.cnt += 1

                cv2.rectangle(image, (x, y), (x + w, y + h), (0, 255, 0), 2)
                cv2.rectangle(image, (x, y), (x + w, y + h), (0, 0, 255), 2)
                

                if x + w/2 > 320:
                    err = x - 200 - 320
                    self.flag = 1 #左转
                else:
                    err = x + w + 200 - 320
                    self.flag = 2 #右转
                #print(f"Zhangaiwu: width={w}, height={h},flagZ={self.flagZ},flag={self.flag},cnt={self.cnt},cnt2={self.cnt2}")
                #err = x - w / 2       #计算打角
                self.twist.linear.x = 0.2
                self.twist.angular.z = -float(err) / 400
                self.cmd_vel_pub.publish(self.twist)
                
        #没有障碍物，开始巡线
        if self.flagZ == 0 and self.cnt < 3:
            
            # if self.cnt5 > 1000:
            #     self.cnt = 0
            #     self.cnt5 = 0
            search_top = 300
            search_bot = 320
            binary_image[0:search_top, 0:640] = 255
            binary_image[search_bot:480, 0:640] = 255
            
            binary_image[search_top:search_bot, 0:90] = 255
            binary_image[search_top:search_bot, 550:w] = 255

            #binary_image3 = cv2.GaussianBlur(binary_image, (7, 7), 1,1)#高斯
        
            # 定义膨胀和腐蚀的结构元素
            dilate_kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (10, 10))
            erode_kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5))
            # 中值滤波
            binary_image3 = cv2.medianBlur(binary_image, 9)

            # 膨胀
            binary_image3 = cv2.dilate(binary_image3, dilate_kernel)

            # 腐蚀
            binary_image3 = cv2.erode(binary_image3, erode_kernel)



            binary_image3 = cv2.bitwise_not(binary_image3)
            contours, _ = cv2.findContours(binary_image3, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)#画框
            # hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)

            # lower_yellow = numpy.array([0, 0, 0])
            # upper_yellow = numpy.array([179, 50, 50])#black
            # mask = cv2.inRange(hsv, lower_yellow, upper_yellow)

            # mask[0:search_top, 0:640] = 0
            # mask[search_bot:480, 0:640] = 0
            # contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)#画框
            #print(f"MianJi---------------------------" )
            #M = cv2.moments(mask)
            #self.pub.publish(self.bridge.cv2_to_imgmsg(binary_image3, 'mono8'))
            #M = cv2.moments(binary_image3)
            #走直线
            

            for contour in contours:
            #if M['m00'] > 0:#能找到黑线    是否要对像素点面积进行限制 以免比较小的黑点造成影响
                
                # 计算轮廓的边界框
                #x = int(M['m10'] / M['m00'])
                #y = int(M['m01'] / M['m00'])
                #cv2.circle(image, (x, y), 20, (0, 0, 255), -1)

                x, y, w, h = cv2.boundingRect(contour)
                # 计算轮廓的面积（像素数量）
                area1 = cv2.contourArea(contour)
                #print(f"MianJi---------------:area={area}" )
                if area1 > 300: 
                    #cx = int(M['m10'] / M['m00'])
                    #cy = int(M['m01'] / M['m00'])
                    cv2.circle(image, (x, y), 20, (0, 0, 255), -1)
                    err = x - 320
                    if x > 300 and x < 340:
                        self.flag = 0
                    # 设置P控制器增益和D控制器增益
                    Kp = 0.9  # P控制增益
                    Kd = 0.07  # D控制增益

                    # 计算角速度指令
                    angular_err = -float(err) / 600
                    d_err = (angular_err - self.last_err) / 0.05  # 计算误差变化率，假设时间间隔为0.1秒
                    self.last_err = angular_err  # 更新上一次的误差

                    # 计算PD控制器的输出
                    control_output = Kp * angular_err + Kd * d_err

                    self.twist.linear.x = 0.2
                    self.twist.angular.z = control_output
                    self.cmd_vel_pub.publish(self.twist)
                    #print(f"Zhixian: err={err},flagZ={self.flagZ},flag={self.flag},cnt={self.cnt},cnt4={self.cnt4}" )
                
                    

        if self.flagZ == 0 and self.cnt >= 3  :
            self.cnt+=1
            self.cnt2 += 1
            #self.cnt5 = 0

            if self.cnt2 <=35 and self.flag == 1 :

                #left
                err = 0 - 640 / 2 #假设线在x = 0处
                self.twist.linear.x = 0.2
                self.twist.angular.z = -float(err) / 500
                self.cmd_vel_pub.publish(self.twist)        # 计算二值化图像的重心              
            if self.cnt2 <=30 and self.flag == 2 :

                #right
                err = 500 - 640 / 2 #假设线在x = 500处
                self.twist.linear.x = 0.2
                self.twist.angular.z = -float(err) / 500
                self.cmd_vel_pub.publish(self.twist)               

            if self.cnt2 >35 and self.cnt2 <= 55 and self.flag == 1:
                #走直线
                self.twist.linear.x = 0.2
                self.twist.angular.z = 0.0
                self.cmd_vel_pub.publish(self.twist)

            elif self.cnt2 >35 and self.cnt2 <= 40:
                #走直线
                self.twist.linear.x = 0.2
                self.twist.angular.z = 0.0
                self.cmd_vel_pub.publish(self.twist)

            if  self.cnt2 >= 80:
                self.cnt2 = 0
                self.cnt = 0
                self.flagZ=0

            if self.flag == 1 and self.cnt2 < 80 and self.cnt2 > 55:# 看到障碍物后往左打 看不到之后往右打
                err = 500 - 640 / 2 #假设线在x = 500处
                self.twist.linear.x = 0.2
                self.twist.angular.z = -float(err) / 600
                self.cmd_vel_pub.publish(self.twist)
                #print(f"RRRRRRRRRRR,cnt2={self.cnt2}")
            elif self.flag == 2 and self.cnt2 < 80 and self.cnt2 > 40:# 看到障碍物后往右打 看不到之后往左打
                err = 0 - 640 / 2 #假设线在x = 0处

                self.twist.linear.x = 0.2
                self.twist.angular.z = -float(err) / 400
                self.cmd_vel_pub.publish(self.twist)        # 计算二值化图像的重心
                #print(f"LLLLLLLLLLL,cnt2={self.cnt2},err={err}")
                
            # 看到障碍物后往左边打 导致看不到黑线 那么黑线再出来的时候 应该从右边出现 如果突然出现应该左边的黑线 他不是我要寻的线

        #M = cv2.moments(binary)
        # 计算重心的x和y坐标
        # if M['m00'] != 0:
        #     cx = int(M['m10'] / M['m00'])
        #     cy = int(M['m01'] / M['m00'])
        #     # 在图像上绘制重心
        #     cv2.circle(image, (cx, cy), 5, (0, 0, 255), -1)
        self.pub.publish(self.bridge.cv2_to_imgmsg(image, 'bgr8'))
        #self.pub.publish(self.bridge.cv2_to_imgmsg(binary_image3, 'mono8'))
        

    def hkh_callback(self, msg):
        if msg.data == 6:  # 假设 1 表示 'OPEN'
            self.flagA += 1
            self.cmd_vel_enabled = True
            print(f"FLAGAAAAAA={self.flagA}")
            self.twist.linear.x = 0.03
            self.twist.angular.z = 0.0
            self.cmd_vel_pub.publish(self.twist)
            self.get_logger().info("Enabled cmd_vel publishing.")
        elif msg.data == 5:  # 假设 0 表示 'OFF'
            self.twist.linear.x = 0.0
            self.twist.angular.z = 0.0
            self.cmd_vel_pub.publish(self.twist)
            self.cmd_vel_enabled = False
            self.get_logger().info("Disabled cmd_vel publishing.")
        else:
            self.get_logger().warning(f"Unknown command received: {msg.data}")


def main(args=None):
    rclpy.init(args=args)
    follower = Follower()
    rclpy.spin(follower)
    follower.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()